#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>

// extra includes
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
//#include <pcl/segmentation/organized_multi_plane_segmentation.h>

//ros::Publisher pub;
ros::Publisher pub1;
ros::Publisher pub2;


void callback (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZRGB> cloud;
  pcl::fromROSMsg (*input, cloud);

  // some clouds
  pcl::PointCloud<pcl::PointXYZRGB> ground_points;
  pcl::PointCloud<pcl::PointXYZRGB> object_points;
  pcl::PointCloud<pcl::PointXYZRGB> cloud_filtered;
  
 //
  pcl::PointCloud<pcl::PointXYZRGB> cloud_p;// (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB> cloud_f;// (new pcl::PointCloud<pcl::PointXYZRGB>);  
  //
  for (pcl::PointCloud<pcl::PointXYZRGB>::iterator p = cloud.begin(); p < cloud.end(); p++) {
  	//std::cerr << "Model inliers: " << p->r << std::endl;
	p->r = 255;
        p->g = 0;
        p->b = 0;
  }  
  // 1. Filter outliers
  
  /*
  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
  sor.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud));
  sor.setMeanK(50);
  sor.setStddevMulThresh(1.0);
  sor.filter(cloud_filtered);
  */
  // 2. Downsample the grid
  
  pcl::VoxelGrid<pcl::PointXYZRGB> downsampler;
  downsampler.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud));//(cloud_filtered));
  downsampler.setLeafSize(0.01, 0.01, 0.01); // leaf size of 1cm
  downsampler.filter(cloud_filtered);

  // 3. Find the ground plane using RANSAC
  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers;
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  seg.setOptimizeCoefficients(true); // Optional
  //  seg.setMaxIterations(500); // Optional, maybe can be lower
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.01);//0.02
  seg.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud_filtered));
  //seg.segment(inliers, coefficients);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  //extract.setKeepOrganized(true);
  int i = 0, nr_points = (int) cloud_filtered.points.size ();
  // While 30% of the original cloud is still there
  while (cloud_filtered.points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud_filtered));
    seg.segment (inliers, coefficients);
    if (inliers.indices.size() <= 100 )//== 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the inliers
    extract.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud_filtered));
    extract.setIndices (boost::make_shared<pcl::PointIndices>(inliers));
    extract.setNegative (false);
    extract.filter (cloud_p);
    std::cerr << "PointCloud representing the planar component: " << cloud_p.width * cloud_p.height << " data points." << std::endl;

    // Create the filtering object
    extract.setNegative (true);
    extract.filter (cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;
  }

  // asd
    extract.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud));
    extract.setNegative (true);
    extract.setKeepOrganized(true);
    extract.filter (cloud_filtered);
    

  sensor_msgs::PointCloud2 ros_cloud1;
  pcl::toROSMsg(cloud, ros_cloud1);

  //sensor_msgs::PointCloud2 ros_cloud2;
  //pcl::toROSMsg(wall_points, ros_cloud2);

  pub1.publish(ros_cloud1);
  //pub2.publish(ros_cloud2);

/*
  // SIMPLER SINGLE PLANE SEGMENTATION
   pcl::ModelCoefficients coefficients;
   pcl::PointIndices inliers;
  // Create the segmentation object
   pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
   seg.setOptimizeCoefficients (true);
  // Mandatory
   seg.setModelType (pcl::SACMODEL_PLANE);
   seg.setMethodType (pcl::SAC_RANSAC);
   seg.setDistanceThreshold (0.01);

  // seg.setInputCloud (cloud.makeShared ());
   seg.segment (inliers, coefficients);
    
   pcl::ExtractIndices<pcl::PointXYZRGB> extract;
   extract.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >(cloud));
   extract.setIndices(boost::make_shared<pcl::PointIndices>(inliers));
   extract.setNegative(false);
   extract.setKeepOrganized(true);
   extract.filter(cloud_filtered);

  
  
  // Publish the model coefficients
   sensor_msgs::PointCloud2 ros_cloud;
   pcl::toROSMsg(cloud_filtered, ros_cloud);
   pub1.publish (ros_cloud);*/
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "planeFilter");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("camera/depth_registered/points", 1, callback);

  // Create a ROS publisher for the output model coefficients
  //pub = nh.advertise<pcl_msgs::ModelCoefficients> ("output1", 1);
  pub1 = nh.advertise<sensor_msgs::PointCloud2> ("filtered_pointcloud", 1);
  //pub2 = nh.advertise<sensor_msgs::PointCloud2> ("output2", 1);

  // Spin
  ros::spin ();
}
